// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include <iostream>
#include "canconfig.h"
#include <unistd.h>

using namespace std::chrono_literals;
// 3.自定义节点类；
class Can_Driver : public rclcpp::Node
{
public:
  Can_Driver() : Node("can_driver_node_cpp")
  {
    // 创建可重入回调组
    // 创建订阅选项，并设置回调组
    // 订阅
    sub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    rclcpp::SubscriptionOptions options_sub;
    options_sub.callback_group = sub_callback_group_;
    // 发布
    pub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    // 定时器
    timer_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

    // 创建速度指令订阅者回调
    this->twist_sub_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&Can_Driver::twistCallback, this, std::placeholders::_1), options_sub);
    // 创建控制线程
    this->speed_sub_ = this->create_subscription<std_msgs::msg::Int32>("speed_true", 10, std::bind(&Can_Driver::speedCallback, this, std::placeholders::_1), options_sub);
    // 创建实时速度发布方
    rtspeed1_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt1_speed", 10);
    rtspeed2_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt2_speed", 10);
    rtspeed3_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt3_speed", 10);
    rtspeed4_pub_ = this->create_publisher<std_msgs::msg::Int32>("rt4_speed", 10);
    // 定时器
    sp_timer_ = this->create_wall_timer(0.01s, std::bind(&Can_Driver::on_rtsp_timer, this), timer_callback_group_);
  }

private:
  // 成员变量
  rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
  rclcpp::CallbackGroup::SharedPtr sub_callback_group_;
  rclcpp::CallbackGroup::SharedPtr pub_callback_group_;
  // 键盘控制节点订阅
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_;
  // float V_x = 0.0; // X轴速度
  // float V_z = 0.0; // Z轴速度（角速度）

  // 速度发布节点订阅
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr speed_sub_;

  // 四个车轮的实时速度反馈
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed1_pub_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed2_pub_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed3_pub_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr rtspeed4_pub_;

  // 定义计时器
  rclcpp::TimerBase::SharedPtr sp_timer_;

  void speedCallback(const std_msgs::msg::Int32::SharedPtr speed_msg)
  {
    V_x = speed_msg->data;
    RCLCPP_INFO(this->get_logger(), "%d", V_x);
  }

  void twistCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
  {
    float Straight_f = msg->linear.x;
    float Turn_f = msg->angular.z;
    if (V_x > 2000)
    {
      // 限制最大速度
      V_x = 2000;
    }

    // 前进
    if (Straight_f > 0)
    {
      // 数据转换（1&3轮）
      // std::cout << "十进制数: " << V_x << std::endl;
      int V_x_1_3 = round(V_x);
      // std::cout << "十进制数00: " << V_x_1_3 << std::endl;
      std::ostringstream stream_1_3;
      stream_1_3 << std::hex << V_x_1_3;
      std::string hexStr_1_3 = stream_1_3.str();
      // std::cout << "十六进制数: " << hexStr_1_3 << std::endl;
      // 左前轮速度(1)
      unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
      output[0] = num_1;
      BYTE speed_data_01[5] = {
          0x1d,
          output[0] & 0xFF,         // 获取最低字节
          (output[0] >> 8) & 0xFF,  // 获取次低字节
          (output[0] >> 16) & 0xFF, // 获取次高字节
          (output[0] >> 24) & 0xFF  // 获取最高字节
      };
      // 左后轮速度(3)
      unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
      output[2] = num_3;
      BYTE speed_data_03[5] = {
          0x1d,
          output[2] & 0xFF,         // 获取最低字节
          (output[2] >> 8) & 0xFF,  // 获取次低字节
          (output[2] >> 16) & 0xFF, // 获取次高字节
          (output[2] >> 24) & 0xFF  // 获取最高字节
      };
      // 数据转换（2&4轮）
      int V_x_2_4 = -round(V_x);
      std::ostringstream stream_2_4;
      stream_2_4 << std::hex << V_x_2_4;
      std::string hexStr_2_4 = stream_2_4.str();
      // 右前轮速度(2)
      unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_2;
      BYTE speed_data_02[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 右前轮速度(4)
      unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_4;
      BYTE speed_data_04[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 发送数据
      SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
      SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
      SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
      SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    }
    // 后退
    if (Straight_f < 0)
    {
      // 数据转换（1&3轮）
      // std::cout << "十进制数: " << V_x << std::endl;
      int V_x_1_3 = -round(V_x);
      // std::cout << "十进制数00: " << V_x_1_3 << std::endl;
      std::ostringstream stream_1_3;
      stream_1_3 << std::hex << V_x_1_3;
      std::string hexStr_1_3 = stream_1_3.str();
      // std::cout << "十六进制数: " << hexStr_1_3 << std::endl;
      // 左前轮速度（1）
      unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
      output[0] = num_1;
      BYTE speed_data_01[5] = {
          0x1d,
          output[0] & 0xFF,         // 获取最低字节
          (output[0] >> 8) & 0xFF,  // 获取次低字节
          (output[0] >> 16) & 0xFF, // 获取次高字节
          (output[0] >> 24) & 0xFF  // 获取最高字节
      };
      // 左后轮速度（3）
      unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
      output[1] = num_3;
      BYTE speed_data_03[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 数据转换（2&4轮）
      int V_x_2_4 = round(V_x);
      std::ostringstream stream_2_4;
      stream_2_4 << std::hex << V_x_2_4;
      std::string hexStr_2_4 = stream_2_4.str();
      // 右前轮速度(2)
      unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_2;
      BYTE speed_data_02[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 右后轮速度(4)
      unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_4;
      BYTE speed_data_04[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };

      // 发送数据
      SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
      SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
      SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
      SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    }
    // 左转
    if (Turn_f > 0)
    {
      // 转向时的直线速度
      int V_x_t = 200;
      // 转向时的旋转速度
      int V_x_f = 200;
      // 数据转换（1&3轮）
      int V_x_1_3 = -round(V_x_t + V_x_f);
      std::ostringstream stream_1_3;
      stream_1_3 << std::hex << V_x_1_3;
      std::string hexStr_1_3 = stream_1_3.str();
      // 左前轮速度（1）
      unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
      output[0] = num_1;
      BYTE speed_data_01[5] = {
          0x1d,
          output[0] & 0xFF,         // 获取最低字节
          (output[0] >> 8) & 0xFF,  // 获取次低字节
          (output[0] >> 16) & 0xFF, // 获取次高字节
          (output[0] >> 24) & 0xFF  // 获取最高字节
      };
      // 左后轮速度（3）
      unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
      output[2] = num_3;
      BYTE speed_data_03[5] = {
          0x1d,
          output[2] & 0xFF,         // 获取最低字节
          (output[2] >> 8) & 0xFF,  // 获取次低字节
          (output[2] >> 16) & 0xFF, // 获取次高字节
          (output[2] >> 24) & 0xFF  // 获取最高字节
      };
      // 数据转换（2&4轮）
      int V_x_2_4 = -round(V_x_t + V_x_f);
      std::ostringstream stream_2_4;
      stream_2_4 << std::hex << V_x_2_4;
      std::string hexStr_2_4 = stream_2_4.str();
      // 右前轮速度(2)
      unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_2;
      BYTE speed_data_02[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 右后轮速度(4)
      unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
      output[3] = num_4;
      BYTE speed_data_04[5] = {
          0x1d,
          output[3] & 0xFF,         // 获取最低字节
          (output[3] >> 8) & 0xFF,  // 获取次低字节
          (output[3] >> 16) & 0xFF, // 获取次高字节
          (output[3] >> 24) & 0xFF  // 获取最高字节
      };
      // 发送数据
      SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
      SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
      SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
      SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    }
    // 右转
    if (Turn_f < 0)
    {
      // 转向时的直线速度
      int V_x_t = 200;
      // 转向时的旋转速度
      int V_x_f = 200;
      // 数据转换（1&3轮）
      int V_x_1_3 = round(V_x_t + V_x_f);
      std::ostringstream stream_1_3;
      stream_1_3 << std::hex << V_x_1_3;
      std::string hexStr_1_3 = stream_1_3.str();
      // 左前轮速度（1）
      unsigned int num_1 = std::stoul(hexStr_1_3, nullptr, 16);
      output[0] = num_1;
      BYTE speed_data_01[5] = {
          0x1d,
          output[0] & 0xFF,         // 获取最低字节
          (output[0] >> 8) & 0xFF,  // 获取次低字节
          (output[0] >> 16) & 0xFF, // 获取次高字节
          (output[0] >> 24) & 0xFF  // 获取最高字节
      };
      // 左后轮速度（3）
      unsigned int num_3 = std::stoul(hexStr_1_3, nullptr, 16);
      output[2] = num_3;
      BYTE speed_data_03[5] = {
          0x1d,
          output[2] & 0xFF,         // 获取最低字节
          (output[2] >> 8) & 0xFF,  // 获取次低字节
          (output[2] >> 16) & 0xFF, // 获取次高字节
          (output[2] >> 24) & 0xFF  // 获取最高字节
      };
      // 数据转换（2&4轮）
      int V_x_2_4 = round(V_x_t + V_x_f);
      std::ostringstream stream_2_4;
      stream_2_4 << std::hex << V_x_2_4;
      std::string hexStr_2_4 = stream_2_4.str();
      // 右前轮速度(2)
      unsigned int num_2 = std::stoul(hexStr_2_4, nullptr, 16);
      output[1] = num_2;
      BYTE speed_data_02[5] = {
          0x1d,
          output[1] & 0xFF,         // 获取最低字节
          (output[1] >> 8) & 0xFF,  // 获取次低字节
          (output[1] >> 16) & 0xFF, // 获取次高字节
          (output[1] >> 24) & 0xFF  // 获取最高字节
      };
      // 右后轮速度(4)
      unsigned int num_4 = std::stoul(hexStr_2_4, nullptr, 16);
      output[3] = num_4;
      BYTE speed_data_04[5] = {
          0x1d,
          output[3] & 0xFF,         // 获取最低字节
          (output[3] >> 8) & 0xFF,  // 获取次低字节
          (output[3] >> 16) & 0xFF, // 获取次高字节
          (output[3] >> 24) & 0xFF  // 获取最高字节
      };
      // 发送数据
      SendData(send_motor_speed, motor_id_01, speed_data_01, 5);
      SendData(send_motor_speed, motor_id_02, speed_data_02, 5);
      SendData(send_motor_speed, motor_id_03, speed_data_03, 5);
      SendData(send_motor_speed, motor_id_04, speed_data_04, 5);
    }

    // 等待毫秒
    std::this_thread::sleep_for(std::chrono::milliseconds(20));
    SendData(send_motor_speed, motor_id_01, speed_zero, 5);
    SendData(send_motor_speed, motor_id_02, speed_zero, 5);
    SendData(send_motor_speed, motor_id_03, speed_zero, 5);
    SendData(send_motor_speed, motor_id_04, speed_zero, 5);
  }

  // 速度反馈函数 此处应该调用回调组
  void on_rtsp_timer()
  {
    rtspeed1_pub_->publish(rt1_data);
    rtspeed2_pub_->publish(rt2_data);
    rtspeed3_pub_->publish(rt3_data);
    rtspeed4_pub_->publish(rt4_data);
    /*
      问题详述：
      如果将查询速度，并且返回速度值，并发布的任务放在这里，会使得键盘控制出现卡顿，因此在下方多线程中实现该功能
      此处仅实现数据发布
    */
  }
};

int main(int argc, char const *argv[])
{

  output[0] = 0;
  output[1] = 0;
  output[2] = 0;
  output[3] = 0;

  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // 初始化CAN设备
  Init_Can();
  // 4.调用spain函数，并传入节点对象指针；
  auto node = std::make_shared<Can_Driver>();
  // 创建多线程执行器（默认线程数 = CPU核心数）
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  // RCLCPP_INFO(node->get_logger(), "Node started. Spinning...");
  executor.spin();
  // 停止电机
  SendData(send_motor_speed, motor_id_01, speed_zero, 5);
  SendData(send_motor_speed, motor_id_02, speed_zero, 5);
  SendData(send_motor_speed, motor_id_03, speed_zero, 5);
  SendData(send_motor_speed, motor_id_04, speed_zero, 5);
  std::this_thread::sleep_for(5s);
  // 速度降低后关闭电机
  SendData(send_motor_speed, motor_id_01, motor_stop, 1);
  SendData(send_motor_speed, motor_id_02, motor_stop, 1);
  SendData(send_motor_speed, motor_id_03, motor_stop, 1);
  SendData(send_motor_speed, motor_id_04, motor_stop, 1);
  // 5.资源释放
  rclcpp::shutdown();
  // 关闭can设备
  VCI_CloseDevice(device_type, device_index);
  return 0;
}

// CAN设备初始化函数
void Init_Can(void)
{
  int m_run0 = 1;
  int ret;
  // 初始化CAN设备
  device_type = VCI_USBCAN2; // 设备类型修改
  device_index = 0;          // 设备的索引
  can_index = 0;             // CAN控制器的索引 CAN1为0，CAN2为1

  if (VCI_OpenDevice(device_type, device_index, 0) != STATUS_OK)
  {
    std::cout << "无法开启CAN设备" << std::endl;
    return;
  }
  else
  {
    std::cout << "设备开启,如需退出请按“CTRL + C”" << std::endl;
  }

  if (VCI_ReadBoardInfo(device_type, device_index, &board_info) != STATUS_OK)
  {
    std::cout << "获取设备信息失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }
  // 设置CAN初始化配置
  init_config.AccCode = 0X00000000;
  init_config.AccMask = 0xFFFFFFFF;
  init_config.Filter = 2; // 只接受标准帧
  init_config.Mode = 0;
  init_config.Timing0 = 0x00;
  init_config.Timing1 = 0x14; // 波特率：1000kps

  if (VCI_InitCAN(device_type, device_index, can_index, &init_config) != STATUS_OK)
  {
    std::cout << "初始化CAN参数失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }

  // 开始CAN通讯
  if (VCI_StartCAN(device_type, device_index, can_index) != STATUS_OK)
  {
    std::cout << "开启CAN通道失败" << std::endl;
    VCI_CloseDevice(device_type, device_index);
    return;
  }

  ret = pthread_create(&threadid, NULL, receive_func, &m_run0);
}

// 指令发送函数
void SendData(VCI_CAN_OBJ &handle_obj, const int id, const BYTE *data, int dataLen)
{
  handle_obj.ID = id;
  handle_obj.RemoteFlag = 0;
  handle_obj.ExternFlag = 0;
  handle_obj.DataLen = dataLen;
  for (int i = 0; i < handle_obj.DataLen; i++)
  {
    handle_obj.Data[i] = data[i];
  }
  if (VCI_Transmit(device_type, device_index, can_index, &handle_obj, 1) > 0)
  {
  }
  else
  {
    std::cout << "初始化错误！" << std::endl;
  }
}

void *receive_func(void *param)
{
  int reclen;
  uint8_t receiveData[5];
  uint8_t realData[2];
  VCI_CAN_OBJ rec[2]; // 接收长度
  (void)param;
  while (rclcpp::ok())
  {
    // std::cout <<"ok"<< std::endl;
    SendData(get_motor_receive, motor_id_01, get_speed, 1);
    SendData(get_motor_receive, motor_id_02, get_speed, 1);
    SendData(get_motor_receive, motor_id_03, get_speed, 1);
    SendData(get_motor_receive, motor_id_04, get_speed, 1);
    // if (reclen = VCI_Receive(device_type, device_index, can_index, rec, Len, WaitTime) > 0)
    if ((reclen = VCI_Receive(device_type, device_index, can_index, rec, Len, WaitTime)) > 0)
    {
      for (int j = 0; j < reclen; j++)
      {
        switch (rec[j].ID)
        {
        case 0x001:
        {
          // int id = rec[j].ID;
          // std::cout << "当前ID是:" << id << std::endl;
          for (int i = 0; i < 5; i++)
          {
            // printf("%02x", rec[j].Data[i]);
            receiveData[i] = rec[j].Data[i];
          }
          realData[0] = receiveData[2];
          realData[1] = receiveData[1];
          int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
          int decval1 = static_cast<int>(hexval);
          // printf("%d", decval);
          // 发布信息
          rt1_data.data = decval1;
          std::cout << "wheel1 == " << decval1 << "\n"
                    << std::endl;
          // rtspeed1_pub_->publish(rt1_data);
          break;
        }

        case 0x002:
        {
          for (int i = 0; i < 5; i++)
          {
            receiveData[i] = rec[j].Data[i];
          }
          realData[0] = receiveData[2];
          realData[1] = receiveData[1];
          int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
          int decval2 = static_cast<int>(hexval);
          // 发布信息
          rt2_data.data = decval2;
          std::cout << "wheel2 == " << decval2 << "\n"
                    << std::endl;
          // std::cout << decval2 << std::endl;
          // rtspeed2_pub_->publish(rt2_data);
          break;
        }

        case 0x003:
        {
          for (int i = 0; i < 5; i++)
          {
            receiveData[i] = rec[j].Data[i];
          }
          realData[0] = receiveData[2];
          realData[1] = receiveData[1];
          int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
          int decval3 = static_cast<int>(hexval);
          // 发布信息
          rt3_data.data = decval3;
          // std::cout << "wheel3 == " << decval3 << "\n"
          //           << std::endl;
          // rtspeed3_pub_->publish(rt3_data);
          break;
        }

        case 0x004:
        {
          for (int i = 0; i < 5; i++)
          {
            receiveData[i] = rec[j].Data[i];
          }
          realData[0] = receiveData[2];
          realData[1] = receiveData[1];
          int16_t hexval = (static_cast<uint16_t>(realData[0] << 8) | realData[1]);
          int decval4 = static_cast<int>(hexval);
          // 发布信息
          rt4_data.data = decval4;
          // std::cout << decval4 << std::endl;
          // rtspeed4_pub_->publish(rt4_data);
          break;
        }
        }
      }
    }
    sleep(0.01); // 10ms延时
  }

  pthread_exit(0);
}
